// I2Cdev library collection - ICM20689 I2C device class
// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
// 8/24/2011 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
//     ... - ongoing debug release

// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE
// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF
// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING.

/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/

#include "ICM20689.h"

/** Default constructor, uses default I2C address.
 * @see ICM20689_DEFAULT_ADDRESS
 */
ICM20689::ICM20689() {
    devAddr = ICM20689_DEFAULT_ADDRESS;
}

/** Specific address constructor.
 * @param address I2C address
 * @see ICM20689_DEFAULT_ADDRESS
 * @see ICM20689_ADDRESS_AD0_LOW
 * @see ICM20689_ADDRESS_AD0_HIGH
 */
ICM20689::ICM20689(uint8_t address) {
    devAddr = address;
}

/** Power on and prepare for general usage.
 * This will activate the device and take it out of sleep mode (which must be done
 * after start-up). This function also sets both the accelerometer and the gyroscope
 * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets
 * the clock source to use the X Gyro for reference, which is slightly better than
 * the default internal clock source.
 */
void ICM20689::initialize() {
    setClockSource(ICM20689_BEST_CLK);
    setFullScaleGyroRange(ICM20689_GYRO_FS_250);
    setFullScaleAccelRange(ICM20689_ACCEL_FS_2);
    setSleepEnabled(false);
}

/** Verify the I2C connection.
 * Make sure the device is connected and responds as expected.
 * @return True if connection is valid, false otherwise
 */
bool ICM20689::testConnection() {
    return getDeviceID() == ICM20689_WHO_AM_I;
}

// SMPLRT_DIV register

/** Get gyroscope output rate divider.
 * The sensor register output, FIFO output, DMP sampling, Motion detection, Zero
 * Motion detection, and Free Fall detection are all based on the Sample Rate.
 * The Sample Rate is generated by dividing the gyroscope output rate by
 * SMPLRT_DIV:
 *
 * Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)
 *
 * where Gyroscope Output Rate = 8kHz when the DLPF is disabled (DLPF_CFG = 0 or
 * 7), and 1kHz when the DLPF is enabled (see Register 26).
 *
 * Note: The accelerometer output rate is 1kHz. This means that for a Sample
 * Rate greater than 1kHz, the same accelerometer sample may be output to the
 * FIFO, DMP, and sensor registers more than once.
 *
 * For a diagram of the gyroscope and accelerometer signal paths, see Section 8
 * of the MPU-6000/MPU-6050 Product Specification document.
 *
 * @return Current sample rate
 * @see ICM20689_RA_SMPLRT_DIV
 */
uint8_t ICM20689::getRate() {
    I2Cdev::readByte(devAddr, ICM20689_RA_SMPLRT_DIV, buffer);
    return buffer[0];
}
/** Set gyroscope sample rate divider.
 * @param rate New sample rate divider
 * @see getRate()
 * @see ICM20689_RA_SMPLRT_DIV
 */
void ICM20689::setRate(uint8_t rate) {
    I2Cdev::writeByte(devAddr, ICM20689_RA_SMPLRT_DIV, rate);
}

// CONFIG register

/** Get external FSYNC configuration.
 * Configures the external Frame Synchronization (FSYNC) pin sampling. An
 * external signal connected to the FSYNC pin can be sampled by configuring
 * EXT_SYNC_SET. Signal changes to the FSYNC pin are latched so that short
 * strobes may be captured. The latched FSYNC signal will be sampled at the
 * Sampling Rate, as defined in register 25. After sampling, the latch will
 * reset to the current FSYNC signal state.
 *
 * The sampled value will be reported in place of the least significant bit in
 * a sensor data register determined by the value of EXT_SYNC_SET according to
 * the following table.
 *
 * <pre>
 * EXT_SYNC_SET | FSYNC Bit Location
 * -------------+-------------------
 * 0            | Input disabled
 * 1            | TEMP_OUT_L[0]
 * 2            | GYRO_XOUT_L[0]
 * 3            | GYRO_YOUT_L[0]
 * 4            | GYRO_ZOUT_L[0]
 * 5            | ACCEL_XOUT_L[0]
 * 6            | ACCEL_YOUT_L[0]
 * 7            | ACCEL_ZOUT_L[0]
 * </pre>
 *
 * @return FSYNC configuration value
 */
uint8_t ICM20689::getExternalFrameSync() {
    I2Cdev::readBits(devAddr, ICM20689_RA_CONFIG, ICM20689_CFG_EXT_SYNC_SET_BIT, ICM20689_CFG_EXT_SYNC_SET_LENGTH, buffer);
    return buffer[0];
}
/** Set external FSYNC configuration.
 * @see getExternalFrameSync()
 * @see ICM20689_RA_CONFIG
 * @param sync New FSYNC configuration value
 */
void ICM20689::setExternalFrameSync(uint8_t sync) {
    I2Cdev::writeBits(devAddr, ICM20689_RA_CONFIG, ICM20689_CFG_EXT_SYNC_SET_BIT, ICM20689_CFG_EXT_SYNC_SET_LENGTH, sync);
}
/** Get digital low-pass filter configuration.
 * The DLPF_CFG parameter sets the digital low pass filter configuration. It
 * also determines the internal sampling rate used by the device as shown in
 * the table below.
 *
 * Note: The accelerometer output rate is 1kHz. This means that for a Sample
 * Rate greater than 1kHz, the same accelerometer sample may be output to the
 * FIFO, DMP, and sensor registers more than once.
 *
 * <pre>
 *          |   ACCELEROMETER    |           GYROSCOPE
 * DLPF_CFG | Bandwidth | Delay  | Bandwidth | Delay  | Sample Rate
 * ---------+-----------+--------+-----------+--------+-------------
 * 0        | 260Hz     | 0ms    | 256Hz     | 0.98ms | 8kHz
 * 1        | 184Hz     | 2.0ms  | 188Hz     | 1.9ms  | 1kHz
 * 2        | 94Hz      | 3.0ms  | 98Hz      | 2.8ms  | 1kHz
 * 3        | 44Hz      | 4.9ms  | 42Hz      | 4.8ms  | 1kHz
 * 4        | 21Hz      | 8.5ms  | 20Hz      | 8.3ms  | 1kHz
 * 5        | 10Hz      | 13.8ms | 10Hz      | 13.4ms | 1kHz
 * 6        | 5Hz       | 19.0ms | 5Hz       | 18.6ms | 1kHz
 * 7        |   -- Reserved --   |   -- Reserved --   | Reserved
 * </pre>
 *
 * @return DLFP configuration
 * @see ICM20689_RA_CONFIG
 * @see ICM20689_CFG_DLPF_CFG_BIT
 * @see ICM20689_CFG_DLPF_CFG_LENGTH
 */
uint8_t ICM20689::getDLPFMode() {
    I2Cdev::readBits(devAddr, ICM20689_RA_CONFIG, ICM20689_CFG_DLPF_CFG_BIT, ICM20689_CFG_DLPF_CFG_LENGTH, buffer);
    return buffer[0];
}
/** Set digital low-pass filter configuration.
 * @param mode New DLFP configuration setting
 * @see getDLPFBandwidth()
 * @see ICM20689_DLPF_BW_256
 * @see ICM20689_RA_CONFIG
 * @see ICM20689_CFG_DLPF_CFG_BIT
 * @see ICM20689_CFG_DLPF_CFG_LENGTH
 */
void ICM20689::setDLPFMode(uint8_t mode) {
    I2Cdev::writeBits(devAddr, ICM20689_RA_CONFIG, ICM20689_CFG_DLPF_CFG_BIT, ICM20689_CFG_DLPF_CFG_LENGTH, mode);
}

// GYRO_CONFIG register

/** Get full-scale gyroscope range.
 * The FS_SEL parameter allows setting the full-scale range of the gyro sensors,
 * as described in the table below.
 *
 * <pre>
 * 0 = +/- 250 degrees/sec
 * 1 = +/- 500 degrees/sec
 * 2 = +/- 1000 degrees/sec
 * 3 = +/- 2000 degrees/sec
 * </pre>
 *
 * @return Current full-scale gyroscope range setting
 * @see ICM20689_GYRO_FS_250
 * @see ICM20689_RA_GYRO_CONFIG
 * @see ICM20689_GCONFIG_FS_SEL_BIT
 * @see ICM20689_GCONFIG_FS_SEL_LENGTH
 */
uint8_t ICM20689::getFullScaleGyroRange() {
    I2Cdev::readBits(devAddr, ICM20689_RA_GYRO_CONFIG, ICM20689_GCONFIG_FS_SEL_BIT, ICM20689_GCONFIG_FS_SEL_LENGTH, buffer);
    return buffer[0];
}
/** Set full-scale gyroscope range.
 * @param range New full-scale gyroscope range value
 * @see getFullScaleRange()
 * @see ICM20689_GYRO_FS_250
 * @see ICM20689_RA_GYRO_CONFIG
 * @see ICM20689_GCONFIG_FS_SEL_BIT
 * @see ICM20689_GCONFIG_FS_SEL_LENGTH
 */
void ICM20689::setFullScaleGyroRange(uint8_t range) {
    I2Cdev::writeBits(devAddr, ICM20689_RA_GYRO_CONFIG, ICM20689_GCONFIG_FS_SEL_BIT, ICM20689_GCONFIG_FS_SEL_LENGTH, range);
}

// SELF TEST FACTORY TRIM VALUES
//TODO

// ACCEL_CONFIG register

/** Get self-test enabled setting for accelerometer X axis.
 * @return Self-test enabled value
 * @see ICM20689_RA_ACCEL_CONFIG
 */
bool ICM20689::getAccelXSelfTest() {
    I2Cdev::readBit(devAddr, ICM20689_RA_ACCEL_CONFIG_1, ICM20689_ACONFIG_XA_ST_BIT, buffer);
    return buffer[0];
}
/** Get self-test enabled setting for accelerometer X axis.
 * @param enabled Self-test enabled value
 * @see ICM20689_RA_ACCEL_CONFIG
 */
void ICM20689::setAccelXSelfTest(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_ACCEL_CONFIG_1, ICM20689_ACONFIG_XA_ST_BIT, enabled);
}
/** Get self-test enabled value for accelerometer Y axis.
 * @return Self-test enabled value
 * @see ICM20689_RA_ACCEL_CONFIG
 */
bool ICM20689::getAccelYSelfTest() {
    I2Cdev::readBit(devAddr, ICM20689_RA_ACCEL_CONFIG_1, ICM20689_ACONFIG_YA_ST_BIT, buffer);
    return buffer[0];
}
/** Get self-test enabled value for accelerometer Y axis.
 * @param enabled Self-test enabled value
 * @see ICM20689_RA_ACCEL_CONFIG
 */
void ICM20689::setAccelYSelfTest(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_ACCEL_CONFIG_1, ICM20689_ACONFIG_YA_ST_BIT, enabled);
}
/** Get self-test enabled value for accelerometer Z axis.
 * @return Self-test enabled value
 * @see ICM20689_RA_ACCEL_CONFIG
 */
bool ICM20689::getAccelZSelfTest() {
    I2Cdev::readBit(devAddr, ICM20689_RA_ACCEL_CONFIG_1, ICM20689_ACONFIG_ZA_ST_BIT, buffer);
    return buffer[0];
}
/** Set self-test enabled value for accelerometer Z axis.
 * @param enabled Self-test enabled value
 * @see ICM20689_RA_ACCEL_CONFIG
 */
void ICM20689::setAccelZSelfTest(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_ACCEL_CONFIG_1, ICM20689_ACONFIG_ZA_ST_BIT, enabled);
}
/** Get full-scale accelerometer range.
 * The FS_SEL parameter allows setting the full-scale range of the accelerometer
 * sensors, as described in the table below.
 *
 * <pre>
 * 0 = +/- 2g
 * 1 = +/- 4g
 * 2 = +/- 8g
 * 3 = +/- 16g
 * </pre>
 *
 * @return Current full-scale accelerometer range setting
 * @see ICM20689_ACCEL_FS_2
 * @see ICM20689_RA_ACCEL_CONFIG
 * @see ICM20689_ACONFIG_AFS_SEL_BIT
 * @see ICM20689_ACONFIG_AFS_SEL_LENGTH
 */
uint8_t ICM20689::getFullScaleAccelRange() {
    I2Cdev::readBits(devAddr, ICM20689_RA_ACCEL_CONFIG_1, ICM20689_ACONFIG_AFS_SEL_BIT, ICM20689_ACONFIG_AFS_SEL_LENGTH, buffer);
    return buffer[0];
}
/** Set full-scale accelerometer range.
 * @param range New full-scale accelerometer range setting
 * @see getFullScaleAccelRange()
 */
void ICM20689::setFullScaleAccelRange(uint8_t range) {
    I2Cdev::writeBits(devAddr, ICM20689_RA_ACCEL_CONFIG_1, ICM20689_ACONFIG_AFS_SEL_BIT, ICM20689_ACONFIG_AFS_SEL_LENGTH, range);
}

// FIFO_EN register

/** Get temperature FIFO enabled value.
 * When set to 1, this bit enables TEMP_OUT_H and TEMP_OUT_L (Registers 65 and
 * 66) to be written into the FIFO buffer.
 * @return Current temperature FIFO enabled value
 * @see ICM20689_RA_FIFO_EN
 */
bool ICM20689::getTempFIFOEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_FIFO_EN, ICM20689_TEMP_FIFO_EN_BIT, buffer);
    return buffer[0];
}
/** Set temperature FIFO enabled value.
 * @param enabled New temperature FIFO enabled value
 * @see getTempFIFOEnabled()
 * @see ICM20689_RA_FIFO_EN
 */
void ICM20689::setTempFIFOEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_FIFO_EN, ICM20689_TEMP_FIFO_EN_BIT, enabled);
}
/** Get gyroscope X-axis FIFO enabled value.
 * When set to 1, this bit enables GYRO_XOUT_H and GYRO_XOUT_L (Registers 67 and
 * 68) to be written into the FIFO buffer.
 * @return Current gyroscope X-axis FIFO enabled value
 * @see ICM20689_RA_FIFO_EN
 */
bool ICM20689::getXGyroFIFOEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_FIFO_EN, ICM20689_XG_FIFO_EN_BIT, buffer);
    return buffer[0];
}
/** Set gyroscope X-axis FIFO enabled value.
 * @param enabled New gyroscope X-axis FIFO enabled value
 * @see getXGyroFIFOEnabled()
 * @see ICM20689_RA_FIFO_EN
 */
void ICM20689::setXGyroFIFOEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_FIFO_EN, ICM20689_XG_FIFO_EN_BIT, enabled);
}
/** Get gyroscope Y-axis FIFO enabled value.
 * When set to 1, this bit enables GYRO_YOUT_H and GYRO_YOUT_L (Registers 69 and
 * 70) to be written into the FIFO buffer.
 * @return Current gyroscope Y-axis FIFO enabled value
 * @see ICM20689_RA_FIFO_EN
 */
bool ICM20689::getYGyroFIFOEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_FIFO_EN, ICM20689_YG_FIFO_EN_BIT, buffer);
    return buffer[0];
}
/** Set gyroscope Y-axis FIFO enabled value.
 * @param enabled New gyroscope Y-axis FIFO enabled value
 * @see getYGyroFIFOEnabled()
 * @see ICM20689_RA_FIFO_EN
 */
void ICM20689::setYGyroFIFOEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_FIFO_EN, ICM20689_YG_FIFO_EN_BIT, enabled);
}
/** Get gyroscope Z-axis FIFO enabled value.
 * When set to 1, this bit enables GYRO_ZOUT_H and GYRO_ZOUT_L (Registers 71 and
 * 72) to be written into the FIFO buffer.
 * @return Current gyroscope Z-axis FIFO enabled value
 * @see ICM20689_RA_FIFO_EN
 */
bool ICM20689::getZGyroFIFOEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_FIFO_EN, ICM20689_ZG_FIFO_EN_BIT, buffer);
    return buffer[0];
}
/** Set gyroscope Z-axis FIFO enabled value.
 * @param enabled New gyroscope Z-axis FIFO enabled value
 * @see getZGyroFIFOEnabled()
 * @see ICM20689_RA_FIFO_EN
 */
void ICM20689::setZGyroFIFOEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_FIFO_EN, ICM20689_ZG_FIFO_EN_BIT, enabled);
}
/** Get accelerometer FIFO enabled value.
 * When set to 1, this bit enables ACCEL_XOUT_H, ACCEL_XOUT_L, ACCEL_YOUT_H,
 * ACCEL_YOUT_L, ACCEL_ZOUT_H, and ACCEL_ZOUT_L (Registers 59 to 64) to be
 * written into the FIFO buffer.
 * @return Current accelerometer FIFO enabled value
 * @see ICM20689_RA_FIFO_EN
 */
bool ICM20689::getAccelFIFOEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_FIFO_EN, ICM20689_ACCEL_FIFO_EN_BIT, buffer);
    return buffer[0];
}
/** Set accelerometer FIFO enabled value.
 * @param enabled New accelerometer FIFO enabled value
 * @see getAccelFIFOEnabled()
 * @see ICM20689_RA_FIFO_EN
 */
void ICM20689::setAccelFIFOEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_FIFO_EN, ICM20689_ACCEL_FIFO_EN_BIT, enabled);
}

// INT_PIN_CFG register

/** Get interrupt logic level mode.
 * Will be set 0 for active-high, 1 for active-low.
 * @return Current interrupt mode (0=active-high, 1=active-low)
 * @see ICM20689_RA_INT_PIN_CFG
 * @see ICM20689_INTCFG_INT_LEVEL_BIT
 */
bool ICM20689::getInterruptMode() {
    I2Cdev::readBit(devAddr, ICM20689_RA_INT_PIN_CFG, ICM20689_INTCFG_INT_LEVEL_BIT, buffer);
    return buffer[0];
}
/** Set interrupt logic level mode.
 * @param mode New interrupt mode (0=active-high, 1=active-low)
 * @see getInterruptMode()
 * @see ICM20689_RA_INT_PIN_CFG
 * @see ICM20689_INTCFG_INT_LEVEL_BIT
 */
void ICM20689::setInterruptMode(bool mode) {
   I2Cdev::writeBit(devAddr, ICM20689_RA_INT_PIN_CFG, ICM20689_INTCFG_INT_LEVEL_BIT, mode);
}
/** Get interrupt drive mode.
 * Will be set 0 for push-pull, 1 for open-drain.
 * @return Current interrupt drive mode (0=push-pull, 1=open-drain)
 * @see ICM20689_RA_INT_PIN_CFG
 * @see ICM20689_INTCFG_INT_OPEN_BIT
 */
bool ICM20689::getInterruptDrive() {
    I2Cdev::readBit(devAddr, ICM20689_RA_INT_PIN_CFG, ICM20689_INTCFG_INT_OPEN_BIT, buffer);
    return buffer[0];
}
/** Set interrupt drive mode.
 * @param drive New interrupt drive mode (0=push-pull, 1=open-drain)
 * @see getInterruptDrive()
 * @see ICM20689_RA_INT_PIN_CFG
 * @see ICM20689_INTCFG_INT_OPEN_BIT
 */
void ICM20689::setInterruptDrive(bool drive) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_INT_PIN_CFG, ICM20689_INTCFG_INT_OPEN_BIT, drive);
}
/** Get interrupt latch mode.
 * Will be set 0 for 50us-pulse, 1 for latch-until-int-cleared.
 * @return Current latch mode (0=50us-pulse, 1=latch-until-int-cleared)
 * @see ICM20689_RA_INT_PIN_CFG
 * @see ICM20689_INTCFG_LATCH_INT_EN_BIT
 */
bool ICM20689::getInterruptLatch() {
    I2Cdev::readBit(devAddr, ICM20689_RA_INT_PIN_CFG, ICM20689_INTCFG_LATCH_INT_EN_BIT, buffer);
    return buffer[0];
}
/** Set interrupt latch mode.
 * @param latch New latch mode (0=50us-pulse, 1=latch-until-int-cleared)
 * @see getInterruptLatch()
 * @see ICM20689_RA_INT_PIN_CFG
 * @see ICM20689_INTCFG_LATCH_INT_EN_BIT
 */
void ICM20689::setInterruptLatch(bool latch) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_INT_PIN_CFG, ICM20689_INTCFG_LATCH_INT_EN_BIT, latch);
}
/** Get interrupt latch clear mode.
 * Will be set 0 for status-read-only, 1 for any-register-read.
 * @return Current latch clear mode (0=status-read-only, 1=any-register-read)
 * @see ICM20689_RA_INT_PIN_CFG
 * @see ICM20689_INTCFG_INT_RD_CLEAR_BIT
 */
bool ICM20689::getInterruptLatchClear() {
    I2Cdev::readBit(devAddr, ICM20689_RA_INT_PIN_CFG, ICM20689_INTCFG_INT_RD_CLEAR_BIT, buffer);
    return buffer[0];
}
/** Set interrupt latch clear mode.
 * @param clear New latch clear mode (0=status-read-only, 1=any-register-read)
 * @see getInterruptLatchClear()
 * @see ICM20689_RA_INT_PIN_CFG
 * @see ICM20689_INTCFG_INT_RD_CLEAR_BIT
 */
void ICM20689::setInterruptLatchClear(bool clear) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_INT_PIN_CFG, ICM20689_INTCFG_INT_RD_CLEAR_BIT, clear);
}
/** Get FSYNC interrupt logic level mode.
 * @return Current FSYNC interrupt mode (0=active-high, 1=active-low)
 * @see getFSyncInterruptMode()
 * @see ICM20689_RA_INT_PIN_CFG
 * @see ICM20689_INTCFG_FSYNC_INT_LEVEL_BIT
 */
bool ICM20689::getFSyncInterruptLevel() {
    I2Cdev::readBit(devAddr, ICM20689_RA_INT_PIN_CFG, ICM20689_INTCFG_FSYNC_INT_LEVEL_BIT, buffer);
    return buffer[0];
}
/** Set FSYNC interrupt logic level mode.
 * @param mode New FSYNC interrupt mode (0=active-high, 1=active-low)
 * @see getFSyncInterruptMode()
 * @see ICM20689_RA_INT_PIN_CFG
 * @see ICM20689_INTCFG_FSYNC_INT_LEVEL_BIT
 */
void ICM20689::setFSyncInterruptLevel(bool level) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_INT_PIN_CFG, ICM20689_INTCFG_FSYNC_INT_LEVEL_BIT, level);
}
/** Get FSYNC pin interrupt enabled setting.
 * Will be set 0 for disabled, 1 for enabled.
 * @return Current interrupt enabled setting
 * @see ICM20689_RA_INT_PIN_CFG
 * @see ICM20689_INTCFG_FSYNC_INT_EN_BIT
 */
bool ICM20689::getFSyncInterruptEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_INT_PIN_CFG, ICM20689_INTCFG_FSYNC_INT_EN_BIT, buffer);
    return buffer[0];
}
/** Set FSYNC pin interrupt enabled setting.
 * @param enabled New FSYNC pin interrupt enabled setting
 * @see getFSyncInterruptEnabled()
 * @see ICM20689_RA_INT_PIN_CFG
 * @see ICM20689_INTCFG_FSYNC_INT_EN_BIT
 */
void ICM20689::setFSyncInterruptEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_INT_PIN_CFG, ICM20689_INTCFG_FSYNC_INT_EN_BIT, enabled);
}

// INT_ENABLE register

// TODO

// INT_STATUS register

/** Get FIFO Buffer Overflow interrupt status.
 * This bit automatically sets to 1 when a Free Fall interrupt has been
 * generated. The bit clears to 0 after the register has been read.
 * @return Current interrupt status
 * @see ICM20689_RA_INT_STATUS
 * @see ICM20689_INTERRUPT_FIFO_OFLOW_BIT
 */
bool ICM20689::getFIFOOverflowInterruptStatus() {
    I2Cdev::readBit(devAddr, ICM20689_RA_INT_STATUS, ICM20689_FIFO_OFLOW_STAT_BIT, buffer);
    return buffer[0];
}
/** Get Data Ready interrupt status.
 * This bit automatically sets to 1 when a Data Ready interrupt has been
 * generated. The bit clears to 0 after the register has been read.
 * @return Current interrupt status
 * @see ICM20689_RA_INT_STATUS
 * @see ICM20689_INTERRUPT_DATA_RDY_BIT
 */
bool ICM20689::getDataReadyInterruptStatus() {
    I2Cdev::readBit(devAddr, ICM20689_RA_INT_STATUS, ICM20689_DATA_RDY_STAT_BIT, buffer);
    return buffer[0];
}

// ACCEL_*OUT_* registers

/** Get raw 6-axis motion sensor readings (accel/gyro).
 * Retrieves all currently available motion sensor values.
 * @param ax 16-bit signed integer container for accelerometer X-axis value
 * @param ay 16-bit signed integer container for accelerometer Y-axis value
 * @param az 16-bit signed integer container for accelerometer Z-axis value
 * @param gx 16-bit signed integer container for gyroscope X-axis value
 * @param gy 16-bit signed integer container for gyroscope Y-axis value
 * @param gz 16-bit signed integer container for gyroscope Z-axis value
 * @see getAcceleration()
 * @see getRotation()
 * @see ICM20689_RA_ACCEL_XOUT_H
 */
void ICM20689::getMotion6(int16_t* ax, int16_t* ay, int16_t* az, int16_t* gx, int16_t* gy, int16_t* gz) {
    I2Cdev::readBytes(devAddr, ICM20689_RA_ACCEL_XOUT_H, 14, buffer);
    *ax = (((int16_t)buffer[0]) << 8) | buffer[1];
    *ay = (((int16_t)buffer[2]) << 8) | buffer[3];
    *az = (((int16_t)buffer[4]) << 8) | buffer[5];
    *gx = (((int16_t)buffer[8]) << 8) | buffer[9];
    *gy = (((int16_t)buffer[10]) << 8) | buffer[11];
    *gz = (((int16_t)buffer[12]) << 8) | buffer[13];
}
/** Get 3-axis accelerometer readings.
 * These registers store the most recent accelerometer measurements.
 * Accelerometer measurements are written to these registers at the Sample Rate
 * as defined in Register 25.
 *
 * The accelerometer measurement registers, along with the temperature
 * measurement registers, gyroscope measurement registers, and external sensor
 * data registers, are composed of two sets of registers: an internal register
 * set and a user-facing read register set.
 *
 * The data within the accelerometer sensors' internal register set is always
 * updated at the Sample Rate. Meanwhile, the user-facing read register set
 * duplicates the internal register set's data values whenever the serial
 * interface is idle. This guarantees that a burst read of sensor registers will
 * read measurements from the same sampling instant. Note that if burst reads
 * are not used, the user is responsible for ensuring a set of single byte reads
 * correspond to a single sampling instant by checking the Data Ready interrupt.
 *
 * Each 16-bit accelerometer measurement has a full scale defined in ACCEL_FS
 * (Register 28). For each full scale setting, the accelerometers' sensitivity
 * per LSB in ACCEL_xOUT is shown in the table below:
 *
 * <pre>
 * AFS_SEL | Full Scale Range | LSB Sensitivity
 * --------+------------------+----------------
 * 0       | +/- 2g           | 8192 LSB/mg
 * 1       | +/- 4g           | 4096 LSB/mg
 * 2       | +/- 8g           | 2048 LSB/mg
 * 3       | +/- 16g          | 1024 LSB/mg
 * </pre>
 *
 * @param x 16-bit signed integer container for X-axis acceleration
 * @param y 16-bit signed integer container for Y-axis acceleration
 * @param z 16-bit signed integer container for Z-axis acceleration
 * @see ICM20689_RA_GYRO_XOUT_H
 */
void ICM20689::getAcceleration(int16_t* x, int16_t* y, int16_t* z) {
    I2Cdev::readBytes(devAddr, ICM20689_RA_ACCEL_XOUT_H, 6, buffer);
    *x = (((int16_t)buffer[0]) << 8) | buffer[1];
    *y = (((int16_t)buffer[2]) << 8) | buffer[3];
    *z = (((int16_t)buffer[4]) << 8) | buffer[5];
}
/** Get X-axis accelerometer reading.
 * @return X-axis acceleration measurement in 16-bit 2's complement format
 * @see getMotion6()
 * @see ICM20689_RA_ACCEL_XOUT_H
 */
int16_t ICM20689::getAccelerationX() {
    I2Cdev::readBytes(devAddr, ICM20689_RA_ACCEL_XOUT_H, 2, buffer);
    return (((int16_t)buffer[0]) << 8) | buffer[1];
}
/** Get Y-axis accelerometer reading.
 * @return Y-axis acceleration measurement in 16-bit 2's complement format
 * @see getMotion6()
 * @see ICM20689_RA_ACCEL_YOUT_H
 */
int16_t ICM20689::getAccelerationY() {
    I2Cdev::readBytes(devAddr, ICM20689_RA_ACCEL_YOUT_H, 2, buffer);
    return (((int16_t)buffer[0]) << 8) | buffer[1];
}
/** Get Z-axis accelerometer reading.
 * @return Z-axis acceleration measurement in 16-bit 2's complement format
 * @see getMotion6()
 * @see ICM20689_RA_ACCEL_ZOUT_H
 */
int16_t ICM20689::getAccelerationZ() {
    I2Cdev::readBytes(devAddr, ICM20689_RA_ACCEL_ZOUT_H, 2, buffer);
    return (((int16_t)buffer[0]) << 8) | buffer[1];
}

// TEMP_OUT_* registers

/** Get current internal temperature.
 * @return Temperature reading in 16-bit 2's complement format
 * @see ICM20689_RA_TEMP_OUT_H
 */
int16_t ICM20689::getTemperature() {
    I2Cdev::readBytes(devAddr, ICM20689_RA_TEMP_OUT_H, 2, buffer);
    return (((int16_t)buffer[0]) << 8) | buffer[1];
}

// GYRO_*OUT_* registers

/** Get 3-axis gyroscope readings.
 * These gyroscope measurement registers, along with the accelerometer
 * measurement registers, temperature measurement registers, and external sensor
 * data registers, are composed of two sets of registers: an internal register
 * set and a user-facing read register set.
 * The data within the gyroscope sensors' internal register set is always
 * updated at the Sample Rate. Meanwhile, the user-facing read register set
 * duplicates the internal register set's data values whenever the serial
 * interface is idle. This guarantees that a burst read of sensor registers will
 * read measurements from the same sampling instant. Note that if burst reads
 * are not used, the user is responsible for ensuring a set of single byte reads
 * correspond to a single sampling instant by checking the Data Ready interrupt.
 *
 * Each 16-bit gyroscope measurement has a full scale defined in FS_SEL
 * (Register 27). For each full scale setting, the gyroscopes' sensitivity per
 * LSB in GYRO_xOUT is shown in the table below:
 *
 * <pre>
 * FS_SEL | Full Scale Range   | LSB Sensitivity
 * -------+--------------------+----------------
 * 0      | +/- 250 degrees/s  | 131 LSB/deg/s
 * 1      | +/- 500 degrees/s  | 65.5 LSB/deg/s
 * 2      | +/- 1000 degrees/s | 32.8 LSB/deg/s
 * 3      | +/- 2000 degrees/s | 16.4 LSB/deg/s
 * </pre>
 *
 * @param x 16-bit signed integer container for X-axis rotation
 * @param y 16-bit signed integer container for Y-axis rotation
 * @param z 16-bit signed integer container for Z-axis rotation
 * @see getMotion6()
 * @see ICM20689_RA_GYRO_XOUT_H
 */
void ICM20689::getRotation(int16_t* x, int16_t* y, int16_t* z) {
    I2Cdev::readBytes(devAddr, ICM20689_RA_GYRO_XOUT_H, 6, buffer);
    *x = (((int16_t)buffer[0]) << 8) | buffer[1];
    *y = (((int16_t)buffer[2]) << 8) | buffer[3];
    *z = (((int16_t)buffer[4]) << 8) | buffer[5];
}
/** Get X-axis gyroscope reading.
 * @return X-axis rotation measurement in 16-bit 2's complement format
 * @see getMotion6()
 * @see ICM20689_RA_GYRO_XOUT_H
 */
int16_t ICM20689::getRotationX() {
    I2Cdev::readBytes(devAddr, ICM20689_RA_GYRO_XOUT_H, 2, buffer);
    return (((int16_t)buffer[0]) << 8) | buffer[1];
}
/** Get Y-axis gyroscope reading.
 * @return Y-axis rotation measurement in 16-bit 2's complement format
 * @see getMotion6()
 * @see ICM20689_RA_GYRO_YOUT_H
 */
int16_t ICM20689::getRotationY() {
    I2Cdev::readBytes(devAddr, ICM20689_RA_GYRO_YOUT_H, 2, buffer);
    return (((int16_t)buffer[0]) << 8) | buffer[1];
}
/** Get Z-axis gyroscope reading.
 * @return Z-axis rotation measurement in 16-bit 2's complement format
 * @see getMotion6()
 * @see ICM20689_RA_GYRO_ZOUT_H
 */
int16_t ICM20689::getRotationZ() {
    I2Cdev::readBytes(devAddr, ICM20689_RA_GYRO_ZOUT_H, 2, buffer);
    return (((int16_t)buffer[0]) << 8) | buffer[1];
}

// SIGNAL_PATH_RESET register

/** Reset accelerometer signal path.
 * The reset will revert the signal path analog to digital converters and
 * filters to their power up configurations.
 * @see ICM20689_RA_SIGNAL_PATH_RESET
 * @see ICM20689_PATHRESET_ACCEL_RESET_BIT
 */
void ICM20689::resetAccelerometerPath() {
    I2Cdev::writeBit(devAddr, ICM20689_RA_SIGNAL_PATH_RESET, ICM20689_PATHRESET_ACCEL_RESET_BIT, true);
}
/** Reset temperature sensor signal path.
 * The reset will revert the signal path analog to digital converters and
 * filters to their power up configurations.
 * @see ICM20689_RA_SIGNAL_PATH_RESET
 * @see ICM20689_PATHRESET_TEMP_RESET_BIT
 */
void ICM20689::resetTemperaturePath() {
    I2Cdev::writeBit(devAddr, ICM20689_RA_SIGNAL_PATH_RESET, ICM20689_PATHRESET_TEMP_RESET_BIT, true);
}

// USER_CTRL register

/** Get FIFO enabled status.
 * When this bit is set to 0, the FIFO buffer is disabled. The FIFO buffer
 * cannot be written to or read from while disabled. The FIFO buffer's state
 * does not change unless the MPU-60X0 is power cycled.
 * @return Current FIFO enabled status
 * @see ICM20689_RA_USER_CTRL
 * @see ICM20689_USERCTRL_FIFO_EN_BIT
 */
bool ICM20689::getFIFOEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_USER_CTRL, ICM20689_USERCTRL_FIFO_EN_BIT, buffer);
    return buffer[0];
}
/** Set FIFO enabled status.
 * @param enabled New FIFO enabled status
 * @see getFIFOEnabled()
 * @see ICM20689_RA_USER_CTRL
 * @see ICM20689_USERCTRL_FIFO_EN_BIT
 */
void ICM20689::setFIFOEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_USER_CTRL, ICM20689_USERCTRL_FIFO_EN_BIT, enabled);
}
/** Switch from I2C to SPI mode (MPU-6000 only)
 * If this is set, the primary SPI interface will be enabled in place of the
 * disabled primary I2C interface.
 */
void ICM20689::switchSPIEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_USER_CTRL, ICM20689_USERCTRL_I2C_IF_DIS_BIT, enabled);
}
/** Reset the FIFO.
 * This bit resets the FIFO buffer when set to 1 while FIFO_EN equals 0. This
 * bit automatically clears to 0 after the reset has been triggered.
 * @see ICM20689_RA_USER_CTRL
 * @see ICM20689_USERCTRL_FIFO_RESET_BIT
 */
void ICM20689::resetFIFO() {
    I2Cdev::writeBit(devAddr, ICM20689_RA_USER_CTRL, ICM20689_USERCTRL_FIFO_RESET_BIT, true);
}
/** Reset all sensor registers and signal paths.
 * When set to 1, this bit resets the signal paths for all sensors (gyroscopes,
 * accelerometers, and temperature sensor). This operation will also clear the
 * sensor registers. This bit automatically clears to 0 after the reset has been
 * triggered.
 *
 * When resetting only the signal path (and not the sensor registers), please
 * use Register 104, SIGNAL_PATH_RESET.
 *
 * @see ICM20689_RA_USER_CTRL
 * @see ICM20689_USERCTRL_SIG_COND_RESET_BIT
 */
void ICM20689::resetSensors() {
    I2Cdev::writeBit(devAddr, ICM20689_RA_USER_CTRL, ICM20689_USERCTRL_SIG_COND_RESET_BIT, true);
}

// PWR_MGMT_1 register

/** Trigger a full device reset.
 * A small delay of ~50ms may be desirable after triggering a reset.
 * @see ICM20689_RA_PWR_MGMT_1
 * @see ICM20689_PWR1_DEVICE_RESET_BIT
 */
void ICM20689::reset() {
    I2Cdev::writeBit(devAddr, ICM20689_RA_PWR_MGMT_1, ICM20689_PWR1_DEVICE_RESET_BIT, true);
}
/** Get sleep mode status.
 * Setting the SLEEP bit in the register puts the device into very low power
 * sleep mode. In this mode, only the serial interface and internal registers
 * remain active, allowing for a very low standby current. Clearing this bit
 * puts the device back into normal mode. To save power, the individual standby
 * selections for each of the gyros should be used if any gyro axis is not used
 * by the application.
 * @return Current sleep mode enabled status
 * @see ICM20689_RA_PWR_MGMT_1
 * @see ICM20689_PWR1_SLEEP_BIT
 */
bool ICM20689::getSleepEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_PWR_MGMT_1, ICM20689_PWR1_SLEEP_SET_BIT, buffer);
    return buffer[0];
}
/** Set sleep mode status.
 * @param enabled New sleep mode enabled status
 * @see getSleepEnabled()
 * @see ICM20689_RA_PWR_MGMT_1
 * @see ICM20689_PWR1_SLEEP_BIT
 */
void ICM20689::setSleepEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_PWR_MGMT_1, ICM20689_PWR1_SLEEP_SET_BIT, enabled);
}
/** Get wake cycle enabled status.
 * When this bit is set to 1 and SLEEP is disabled, the MPU-60X0 will cycle
 * between sleep mode and waking up to take a single sample of data from active
 * sensors at a rate determined by LP_WAKE_CTRL (register 108).
 * @return Current sleep mode enabled status
 * @see ICM20689_RA_PWR_MGMT_1
 * @see ICM20689_PWR1_CYCLE_BIT
 */
bool ICM20689::getAccelWakeCycleEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_PWR_MGMT_1, ICM20689_PWR1_ACCEL_CYCLE_SET_BIT, buffer);
    return buffer[0];
}
/** Set wake cycle enabled status.
 * @param enabled New sleep mode enabled status
 * @see getWakeCycleEnabled()
 * @see ICM20689_RA_PWR_MGMT_1
 * @see ICM20689_PWR1_CYCLE_BIT
 */
void ICM20689::setAccelWakeCycleEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_PWR_MGMT_1, ICM20689_PWR1_ACCEL_CYCLE_SET_BIT, enabled);
}
/** Get temperature sensor enabled status.
 * Control the usage of the internal temperature sensor.
 *
 * Note: this register stores the *disabled* value, but for consistency with the
 * rest of the code, the function is named and used with standard true/false
 * values to indicate whether the sensor is enabled or disabled, respectively.
 *
 * @return Current temperature sensor enabled status
 * @see ICM20689_RA_PWR_MGMT_1
 * @see ICM20689_PWR1_TEMP_DISABLE_BIT
 */
bool ICM20689::getTempSensorEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_PWR_MGMT_1, ICM20689_PWR1_TEMP_DISABLE_BIT, buffer);
    return buffer[0] == 0; // 1 is actually disabled here
}
/** Set temperature sensor enabled status.
 * Note: this register stores the *disabled* value, but for consistency with the
 * rest of the code, the function is named and used with standard true/false
 * values to indicate whether the sensor is enabled or disabled, respectively.
 *
 * @param enabled New temperature sensor enabled status
 * @see getTempSensorEnabled()
 * @see ICM20689_RA_PWR_MGMT_1
 * @see ICM20689_PWR1_TEMP_DISABLE_BIT
 */
void ICM20689::setTempSensorEnabled(bool enabled) {
    // 1 is actually disabled here
    I2Cdev::writeBit(devAddr, ICM20689_RA_PWR_MGMT_1, ICM20689_PWR1_TEMP_DISABLE_BIT, !enabled);
}
/** Get clock source setting.
 * @return Current clock source setting
 * @see ICM20689_RA_PWR_MGMT_1
 * @see ICM20689_PWR1_CLKSEL_BIT
 * @see ICM20689_PWR1_CLKSEL_LENGTH
 */
uint8_t ICM20689::getClockSource() {
    I2Cdev::readBits(devAddr, ICM20689_RA_PWR_MGMT_1, ICM20689_PWR1_CLKSEL_BIT, ICM20689_PWR1_CLKSEL_LENGTH, buffer);
    return buffer[0];
}
/** Set clock source setting.
 * An internal 8MHz oscillator, gyroscope based clock, or external sources can
 * be selected as the MPU-60X0 clock source. When the internal 8 MHz oscillator
 * or an external source is chosen as the clock source, the MPU-60X0 can operate
 * in low power modes with the gyroscopes disabled.
 *
 * Upon power up, the MPU-60X0 clock source defaults to the internal oscillator.
 * However, it is highly recommended that the device be configured to use one of
 * the gyroscopes (or an external clock source) as the clock reference for
 * improved stability. The clock source can be selected according to the following table:
 *
 * <pre>
 * CLK_SEL | Clock Source
 * --------+--------------------------------------
 * 0       | Internal oscillator
 * 1       | PLL with X Gyro reference
 * 2       | PLL with Y Gyro reference
 * 3       | PLL with Z Gyro reference
 * 4       | PLL with external 32.768kHz reference
 * 5       | PLL with external 19.2MHz reference
 * 6       | Reserved
 * 7       | Stops the clock and keeps the timing generator in reset
 * </pre>
 *
 * @param source New clock source setting
 * @see getClockSource()
 * @see ICM20689_RA_PWR_MGMT_1
 * @see ICM20689_PWR1_CLKSEL_BIT
 * @see ICM20689_PWR1_CLKSEL_LENGTH
 */
void ICM20689::setClockSource(uint8_t source) {
    I2Cdev::writeBits(devAddr, ICM20689_RA_PWR_MGMT_1, ICM20689_PWR1_CLKSEL_BIT, ICM20689_PWR1_CLKSEL_LENGTH, source);
}

// PWR_MGMT_2 register

/** Get X-axis accelerometer standby enabled status.
 * If enabled, the X-axis will not gather or report data (or use power).
 * @return Current X-axis standby enabled status
 * @see ICM20689_RA_PWR_MGMT_2
 * @see ICM20689_PWR2_STBY_XA_BIT
 */
bool ICM20689::getStandbyXAccelEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_PWR_MGMT_2, ICM20689_PWR2_STBY_XA_BIT, buffer);
    return buffer[0];
}
/** Set X-axis accelerometer standby enabled status.
 * @param New X-axis standby enabled status
 * @see getStandbyXAccelEnabled()
 * @see ICM20689_RA_PWR_MGMT_2
 * @see ICM20689_PWR2_STBY_XA_BIT
 */
void ICM20689::setStandbyXAccelEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_PWR_MGMT_2, ICM20689_PWR2_STBY_XA_BIT, enabled);
}
/** Get Y-axis accelerometer standby enabled status.
 * If enabled, the Y-axis will not gather or report data (or use power).
 * @return Current Y-axis standby enabled status
 * @see ICM20689_RA_PWR_MGMT_2
 * @see ICM20689_PWR2_STBY_YA_BIT
 */
bool ICM20689::getStandbyYAccelEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_PWR_MGMT_2, ICM20689_PWR2_STBY_YA_BIT, buffer);
    return buffer[0];
}
/** Set Y-axis accelerometer standby enabled status.
 * @param New Y-axis standby enabled status
 * @see getStandbyYAccelEnabled()
 * @see ICM20689_RA_PWR_MGMT_2
 * @see ICM20689_PWR2_STBY_YA_BIT
 */
void ICM20689::setStandbyYAccelEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_PWR_MGMT_2, ICM20689_PWR2_STBY_YA_BIT, enabled);
}
/** Get Z-axis accelerometer standby enabled status.
 * If enabled, the Z-axis will not gather or report data (or use power).
 * @return Current Z-axis standby enabled status
 * @see ICM20689_RA_PWR_MGMT_2
 * @see ICM20689_PWR2_STBY_ZA_BIT
 */
bool ICM20689::getStandbyZAccelEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_PWR_MGMT_2, ICM20689_PWR2_STBY_ZA_BIT, buffer);
    return buffer[0];
}
/** Set Z-axis accelerometer standby enabled status.
 * @param New Z-axis standby enabled status
 * @see getStandbyZAccelEnabled()
 * @see ICM20689_RA_PWR_MGMT_2
 * @see ICM20689_PWR2_STBY_ZA_BIT
 */
void ICM20689::setStandbyZAccelEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_PWR_MGMT_2, ICM20689_PWR2_STBY_ZA_BIT, enabled);
}
/** Get X-axis gyroscope standby enabled status.
 * If enabled, the X-axis will not gather or report data (or use power).
 * @return Current X-axis standby enabled status
 * @see ICM20689_RA_PWR_MGMT_2
 * @see ICM20689_PWR2_STBY_XG_BIT
 */
bool ICM20689::getStandbyXGyroEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_PWR_MGMT_2, ICM20689_PWR2_STBY_XG_BIT, buffer);
    return buffer[0];
}
/** Set X-axis gyroscope standby enabled status.
 * @param New X-axis standby enabled status
 * @see getStandbyXGyroEnabled()
 * @see ICM20689_RA_PWR_MGMT_2
 * @see ICM20689_PWR2_STBY_XG_BIT
 */
void ICM20689::setStandbyXGyroEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_PWR_MGMT_2, ICM20689_PWR2_STBY_XG_BIT, enabled);
}
/** Get Y-axis gyroscope standby enabled status.
 * If enabled, the Y-axis will not gather or report data (or use power).
 * @return Current Y-axis standby enabled status
 * @see ICM20689_RA_PWR_MGMT_2
 * @see ICM20689_PWR2_STBY_YG_BIT
 */
bool ICM20689::getStandbyYGyroEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_PWR_MGMT_2, ICM20689_PWR2_STBY_YG_BIT, buffer);
    return buffer[0];
}
/** Set Y-axis gyroscope standby enabled status.
 * @param New Y-axis standby enabled status
 * @see getStandbyYGyroEnabled()
 * @see ICM20689_RA_PWR_MGMT_2
 * @see ICM20689_PWR2_STBY_YG_BIT
 */
void ICM20689::setStandbyYGyroEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_PWR_MGMT_2, ICM20689_PWR2_STBY_YG_BIT, enabled);
}
/** Get Z-axis gyroscope standby enabled status.
 * If enabled, the Z-axis will not gather or report data (or use power).
 * @return Current Z-axis standby enabled status
 * @see ICM20689_RA_PWR_MGMT_2
 * @see ICM20689_PWR2_STBY_ZG_BIT
 */
bool ICM20689::getStandbyZGyroEnabled() {
    I2Cdev::readBit(devAddr, ICM20689_RA_PWR_MGMT_2, ICM20689_PWR2_STBY_ZG_BIT, buffer);
    return buffer[0];
}
/** Set Z-axis gyroscope standby enabled status.
 * @param New Z-axis standby enabled status
 * @see getStandbyZGyroEnabled()
 * @see ICM20689_RA_PWR_MGMT_2
 * @see ICM20689_PWR2_STBY_ZG_BIT
 */
void ICM20689::setStandbyZGyroEnabled(bool enabled) {
    I2Cdev::writeBit(devAddr, ICM20689_RA_PWR_MGMT_2, ICM20689_PWR2_STBY_ZG_BIT, enabled);
}

// FIFO_COUNT* registers

/** Get current FIFO buffer size.
 * This value indicates the number of bytes stored in the FIFO buffer. This
 * number is in turn the number of bytes that can be read from the FIFO buffer
 * and it is directly proportional to the number of samples available given the
 * set of sensor data bound to be stored in the FIFO (register 35 and 36).
 * @return Current FIFO buffer size
 */
uint16_t ICM20689::getFIFOCount() {
    I2Cdev::readBytes(devAddr, ICM20689_RA_FIFO_COUNTH, 2, buffer);
    return (((uint16_t)buffer[0]) << 8) | buffer[1];
}

// FIFO_R_W register

/** Get byte from FIFO buffer.
 * This register is used to read and write data from the FIFO buffer. Data is
 * written to the FIFO in order of register number (from lowest to highest). If
 * all the FIFO enable flags (see below) are enabled and all External Sensor
 * Data registers (Registers 73 to 96) are associated with a Slave device, the
 * contents of registers 59 through 96 will be written in order at the Sample
 * Rate.
 *
 * The contents of the sensor data registers (Registers 59 to 96) are written
 * into the FIFO buffer when their corresponding FIFO enable flags are set to 1
 * in FIFO_EN (Register 35). An additional flag for the sensor data registers
 * associated with I2C Slave 3 can be found in I2C_MST_CTRL (Register 36).
 *
 * If the FIFO buffer has overflowed, the status bit FIFO_OFLOW_INT is
 * automatically set to 1. This bit is located in INT_STATUS (Register 58).
 * When the FIFO buffer has overflowed, the oldest data will be lost and new
 * data will be written to the FIFO.
 *
 * If the FIFO buffer is empty, reading this register will return the last byte
 * that was previously read from the FIFO until new data is available. The user
 * should check FIFO_COUNT to ensure that the FIFO buffer is not read when
 * empty.
 *
 * @return Byte from FIFO buffer
 */
uint8_t ICM20689::getFIFOByte() {
    I2Cdev::readByte(devAddr, ICM20689_RA_FIFO_R_W, buffer);
    return buffer[0];
}
void ICM20689::getFIFOBytes(uint8_t *data, uint8_t length) {
    if(length > 0){
        I2Cdev::readBytes(devAddr, ICM20689_RA_FIFO_R_W, length, data);
    } else {
    	*data = 0;
    }
}
/** Write byte to FIFO buffer.
 * @see getFIFOByte()
 * @see ICM20689_RA_FIFO_R_W
 */
void ICM20689::setFIFOByte(uint8_t data) {
    I2Cdev::writeByte(devAddr, ICM20689_RA_FIFO_R_W, data);
}

// WHO_AM_I register

/** Get Device ID.
 * This register is used to verify the identity of the device (0b110100, 0x34).
 * @return Device ID
 * @see ICM20689_RA_WHO_AM_I
 */
uint8_t ICM20689::getDeviceID() {
    I2Cdev::readByte(devAddr, ICM20689_RA_WHO_AM_I, buffer);
    return buffer[0];
}
/** Set Device ID.
 * Write a new ID into the WHO_AM_I register (no idea why this should ever be
 * necessary though).
 * @param id New device ID to set.
 * @see getDeviceID()
 * @see ICM20689_RA_WHO_AM_I
 * @see ICM20689_WHO_AM_I_BIT
 * @see ICM20689_WHO_AM_I_LENGTH
 */
void ICM20689::setDeviceID(uint8_t id) {
    I2Cdev::writeByte(devAddr, ICM20689_RA_WHO_AM_I, id);
}